/***************************************************************************
 *   Copyright (C) 2010 by Oleg Goncharov  *
 *   $EMAIL$                           *                          
 *                                                                         *
 *   This file is part of ChessVision.                                     *
 *                                                                         *
 *   ChessVision is free software; you can redistribute it and/or modify   *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <iostream>
#include <opencv/cv.h>
#include <opencv/highgui.h>


#include "../error/cerror.h"
#include "../vision/cimagefromfile.h"
#include "../vision/cimagecamthread.h"
#include "../katanavision/ckatanawitheye.h"
#include "../katanavision/katanafuncs.h"
#include "chtransdetector.h"

#include "../vision/cpointmaxcolor.h"
#include "../vision/cpointcirclecenter.h"
#include "../vision/cpointmatch.h"
#include "../vision/cpointbackhist.h"
#include "cgnuchessenvelop.h"

using namespace std;
using namespace Chess;
using namespace Katana;

// 0 GNUCHESSENVELOP TEST 
int main0() {
	SMove mv;
	try {
			CGnuchessEnvelop chess("hoichess");
			chess.Move(SMove("e2e4"));
			chess.Go(mv);
			cout << string(mv) << endl;
			chess.Move(SMove("e2e4"));
			chess.Go(mv);
			cout << string(mv) << endl;
	}
	catch ( Errors::CError& e ) {
		e.Report(std::cout);
	}
	return 0;
}

void MakeMoveS(CKatanaSensing& katana, const cv::Point3f& from, const  cv::Point3f to) {
	double tablez = -186.5;
	double fheight = 75.0;
	
	katana.movDegrees(M_GRP, 5, true);
	katana.moveRobotTo(from.x, from.y, tablez + fheight + 15, 0,0,0, true);
	FindObjectInPlane(katana, 30, 5, 10, 5000);
	GrabDotObject(katana, 20, 20, 5000); 
	katana.moveRobotTo(from.x, from.y, tablez + 2.5*fheight + 10, 0,0,0, true);
	katana.moveRobotTo(to.x, to.y, tablez + 2.5*fheight + 10, 0,0,0, true);
	katana.moveRobotTo(to.x, to.y, tablez + fheight, 0,0,0, true);
	katana.movDegrees(M_GRP, 20, true);
}

void MakeMove(CikBase& katana, const cv::Point3f& from, cv::Point3f to) {
	double tablez = -186.5;
	double fheight = 76.0;
	SActuatorPosition p;
	
	cout << "Move figure from " << from << " to " << to << endl;;
	
	KNI::KatanaKinematics::encoders enc(5, 0);
	KNI::KatanaKinematics::coordinates pose(6, 0);
	double phi1, delta, r;
	
	pose[0] = from.x; pose[1] = from.y; pose[2] = from.z + fheight;
	pose[3] = 0; pose[4] = 0; pose[5] = 0;
	katana.getKinematics().IK(enc.begin(), pose, enc);
	katana.getKinematics().DK(pose, enc);
	phi1 = pose[3];
	
	cv::Point3f go = to;
	double error, r_target =  sqrt(to.x*to.x + to.y*to.y);
	do {
		pose[0] = go.x; pose[1] = go.y; pose[2] = go.z + fheight;
		pose[3] = 0; pose[4] = 0; pose[5] = 0;
		katana.getKinematics().IK(enc.begin(), pose, enc);
		katana.getKinematics().DK(pose, enc);

		delta = pose[3] - phi1;
		r = sqrt(go.x*go.x + go.y*go.y) + fheight*delta;
		error = r_target - r;
		cout << " Err = " << error << endl;
		delta = (r + 0.1*error)/r;
		go.x = go.x*delta; 
		go.y = go.y*delta;
		cout << " Go = " << go << endl;
	} while (fabs(error) > 3);
		
	cout << " { " << to << " } " << endl;
	
		katana.moveRobotTo(from.x, from.y, tablez + 2*fheight + 30, 0,0,0, true);
	katana.moveRobotTo(from.x, from.y, tablez + fheight + 15, 0,0,0, true);
	GrabDotObject(katana, 15, 15, 5000); 
	
	katana.getCoordinates(p.x,p.y,p.z,p.phi,p.theta,p.psi);
	std::cout << "phi = " << p.phi << std::endl; 
	
	katana.moveRobotTo(from.x, from.y, tablez + 2*fheight + 30, 0,0,0, true);
	katana.moveRobotTo(to.x, to.y, tablez + 2*fheight + 30, 0,0,0, true);
	
	katana.moveRobotTo(to.x, to.y, tablez + fheight, 0,0,0, true);
	katana.movDegrees(M_GRP, 15, true);
	
	katana.getCoordinates(p.x,p.y,p.z,p.phi,p.theta,p.psi);
	std::cout << "phi = " << p.phi << std::endl; 
	
	katana.moveRobotTo(to.x, to.y, tablez + 2*fheight + 30, 0,0,0, true);
}

// 1 MOVE TEST
int main( int argc, char *argv[] )
{	
	try {
		TCdlCOMDesc ccd = {0, 57600, 8, 'N', 1, 300, 0};
		CCdlCOM device(ccd);
		CCplSerialCRC protocol;
		protocol.init(&device);
		CikBase katana;
		katana.create ( "katana5M180.cfg", &protocol );
		
		/*katana.moveRobotTo(300, 0, -100, 0,0,0, true);
		katana.UpdateSensors();
		katana.movDegrees(M_GRP, 5, true);
		katana.UpdateSensors();
		katana.UpdateSensors();
		katana.movDegrees(M_GRP, 50, true);
		katana.CalculateNoObstacleValues(10);*/
		
		MakeMove(katana, cv::Point3f(300,0,-186.5), cv::Point3f(500,0,-186.5));
	} 
	catch ( Exception &e ) {
		std::cout << "ERROR: " << e.message() << std::endl;
		return -1;
	}
	catch ( Errors::CError& e ) {
		e.Report(std::cout);
	}
}

// 2 : CELL MOVE TEST
int main2( int argc, char *argv[] )
{	
	try {
		TCdlCOMDesc ccd = {0, 57600, 8, 'N', 1, 300, 0};
		CCdlCOM device(ccd);
		CCplSerialCRC protocol;
		protocol.init(&device);
		Katana::CKatanaSensing katana;
		katana.create ( "katana5M180.cfg", &protocol );
		
		katana.movDegrees(M_GRP, 5, true);
		katana.moveRobotTo(0, 400, 0, 0,0,0, true);
		
		cvNamedWindow("win", 0);
		//cvSetMouseCallback( "win", on_mouse, 0 );
		
		CImageCamThread cam(0, 50);
		//CImageDvGrab cam("--jpeg-quality 100");
		
		CCamTransform camtr("calibdata.xml");
		camtr.LoadTransform("transform.xml");
		
		CHTransDetector det;
		det.SetCannyThereholds(30,70);
		det.SetSteps(0.7f, 0.01f);
		cam.GrabFrame();
		CBoardCells cells;
		while (!det.DetectChessboard(cam.Img(), cells)) { cam.GrabFrame(); };
		
		cells.DrawCells(cam.Img());
		cvShowImage("win", cam.Img());
		cvWaitKey(2000);
		
		cv::Point3f to;
		double tablez = -186.5;
		char c;
		
		to =  camtr.ReverseProjection(cells.Cell(0), HPlane3f(0,0,-1,tablez));
		cout << "Move to " << to << "." << endl;
		katana.moveRobotTo(to.x, to.y, tablez + 20, 0, 0, 0, true);
		cin >> c;
		to =  camtr.ReverseProjection(cells.Cell(7), HPlane3f(0,0,-1,tablez));
		cout << "Move to " << to << "." << endl;
		katana.moveRobotTo(to.x, to.y, tablez + 20, 0, 0, 0, true);
		cin >> c;
		to =  camtr.ReverseProjection(cells.Cell(56), HPlane3f(0,0,-1,tablez));
		cout << "Move to " << to << "." << endl;
		katana.moveRobotTo(to.x, to.y, tablez + 20, 0, 0, 0, true);
		cin >> c;
		to =  camtr.ReverseProjection(cells.Cell(63), HPlane3f(0,0,-1,tablez));
		cout << "Move to " << to << "." << endl;
		katana.moveRobotTo(to.x, to.y, tablez + 20, 0, 0, 0, true);
	} 
	catch ( Exception &e ) {
		std::cout << "ERROR: " << e.message() << std::endl;
		return -1;
	}
	catch ( Errors::CError& e ) {
		e.Report(std::cout);		
	}
}

// 3 : BOARD DETECTOR TEST
int main3( int argc, char *argv[] )
{	
	try {
		IplImage * win_img = 0;
		cvNamedWindow("win", 0);
		
		//CImageCamera cam(0);
		//CImageDvGrab cam("--jpeg-quality 100");
		CImageCamThread cam(0, 50);
		//CImageFromFile cam("../../imgs/chess2.jpg");
		
		CHTransDetector det;
		det.SetCannyThereholds(30,70);
		//CHoughDetector det;
		//det.SetStep_h(1);
		//det.SetStep_teta(0.05);
		det.SetSteps(0.7f, 0.005f);
		det.SetTweak(0.03f, 10.f);
		
		cout << "Performing board detection..." << endl;
		CBoardCells cells;
		if (det.DetectChessboard(cam.Img(), cells)) {
			cout << "Board has been detected." << endl;
			
			win_img = cvCloneImage(cam.Img());
			cells.DrawCells(win_img);
			cvShowImage("win", win_img);
			cvWaitKey(-1);
		}
		else cout << "Board has NOT been detected." << endl;		
	} 
	catch ( Exception &e ) {
		std::cout << "ERROR: " << e.message() << std::endl;
		return -1;
	}
	catch ( Errors::CError& e ) {
		e.Report(std::cout);		
	}
}

